package edu.wpi.first.wpilibj.templates;

import com.sun.squawk.util.MathUtils;
import java.lang.Math.*;

public class FBW
{
    //The velocity of the motor for the ball shooter m/s
    final private static double SHOOTER_VELOCITY = 1;
    //The height of the top basket from the floor (meters)
    final private static double TARGET_HEIGHT = 2.7178;
    //Gravity constant meters per second^2
    final private static double GRAVITY = 9.81;

    // Value to set the ball transport system motors
    public double motorSystemVal = 0.9;

    public FBW() {
        //Constructor for Fly By Wire... Nothing needed here I'm assuming.
    }

    public double[] processVals(double yLeft, double yRight) {
        //Take joystick values and process them into motor inputs.
        if (yLeft < 0) {
            yLeft = yLeft*yLeft*-1;
        } else {
            yLeft = yLeft*yLeft;
        }

        if (yRight < 0) {
            yRight = yRight*yRight*-1;
        } else {
            yRight = yRight*yRight;
        }

        yLeft *= 0.9 * 0.8;
        yRight *= -0.9 * 0.8;
        double[] toMotors = {yLeft, yRight};

        return toMotors;
    }

    public double calcRange(int rawRange) {
        //return (double) rawRange;

        double returnAmount;
        returnAmount= 1.9 * rawRange - 9.27;
        return returnAmount * (72.0/250.0);


   }
    //Calculations to figure out the optimum angle of fire
    public double ShooterAngle(double distance) {
          double theta;
          theta = Math.sqrt(MathUtils.pow(SHOOTER_VELOCITY, 4.0)- GRAVITY*(GRAVITY*MathUtils.pow(distance, 2.0) + 2*TARGET_HEIGHT*MathUtils.pow(SHOOTER_VELOCITY, 2.0)));
          theta = MathUtils.pow(SHOOTER_VELOCITY, 2) + theta;
          theta = theta / (GRAVITY*distance);
          theta = MathUtils.atan(theta);
          return theta;
    }

    public double[] trackBall(boolean enable, int leftDistance, int rightDistance){
        double[] retVals = new double[2];
        if(enable == true){

            if(leftDistance < 50){
                retVals[0] = -.5;
                retVals[1] = 0.5;
            }
            else if (rightDistance < 50){
                retVals[0] = 0.5;
                retVals[1] = 1.5;
            }
            else{
                retVals[0] = 0.5;
                retVals[1] = 0.5;
            }
        }
        else{
                retVals[0] = -1;
                retVals[1] = 1;
        }
    return retVals;
    }
    public double processTowerMotor(double initVal){
        //Control tower motors to move balls to shooter
        return initVal;
    }
    public double proccessPusherMotor(double pusherVal){
        //Control motor that pushes balls
        return pusherVal;
    }
    public double processRollerMotor(double rollerVal){
        //Control the motor that sucks up balls
        return rollerVal;
    }

    /**
     * Calculate the bass positions based on where we currently have balls
     * @param ballPos [pos1, pos2, pos3], booleans, do we see a ball?
     *      assumes:    1) low
     *                  2) mid
     *                  3) top
     * @return motors [feeder, bottom, mid, top] motor vals
     */
    public double[] calcBallSystemVals(boolean[] ballPos) {
        double[] retVals = new double[4];

        retVals[0] = ballPos[0] && ballPos[1] && ballPos[2] ? -1 * motorSystemVal : motorSystemVal;
        retVals[1] = ballPos[0] && ballPos[1] && ballPos[2] ? 0 : motorSystemVal;
        retVals[2] = ballPos[1] && ballPos[2] ? 0 : motorSystemVal;
        retVals[3] = ballPos[2] ? 0 : motorSystemVal;

        return retVals;
    }
    public double setRampPower(double in){
        return 0.9*in;
    }
}
